#include "microbit.h"
#include "uart.h"
#include <math.h>
#include "stateControl.h"
#include "positionPid.h"
#include "mission.h"
#include "stateEstimator.h"
#include "battery.h"


void recv(uint8_t *data, uint8_t len);
void updateMicrobitData(uint8_t data);

//static bool isInit;
static bool needXFlag = false;
static bool needYFlag = false;
static bool needZFlag = false;
static bool needXYZFlag = false;
static bool needFlipFlag = false;
static bool needWaitTimeFlag = false;
static bool needYawFlag = false;
static bool needTakeOffFlag = false;
static bool needLandFlag = false;
static uint32_t cmdLiveCount = 0;
static uint8_t waitTime = 0;

static uint8_t receivedHeart = 0;
static uint16_t rIndex = 0;
static uint16_t wIndex = 0;

//static uint8_t flipFailCount = 0;
//extern bool isExitFlip; /*是否退出空翻*/
/**
void microbitInit(void)
{
    if (isInit)
		return;
	uartMicrobitInit();
    isInit = true;
}
**/
static void runCmd(uint8_t cmd, uint8_t *data)
{
    uint8_t recvData[3] = {1, 1, 0};
    cmdLiveCount = 0;
    switch (cmd)
    {
    case 1: //起飞			
		setArmFlag(1);		
		setFlightState(CONTROL_MODE_TAKEOFF);	//CONTROL_MODE_STABILIZE  CONTROL_MODE_TAKEOFF		
        needTakeOffFlag = true;
		break;
    case 2: //降落
		setFlightState(CONTROL_MODE_LAND);	
        needLandFlag = true;
        break;
    case 3: //速度
        setPosOutPutCoff(data[0] * 0.01f);
		//setVelIsOkFlag(0);
        recv(recvData, 3);
        break;
    case 4: //等待多少秒
        waitTime = data[0];
        needWaitTimeFlag = true;
        break;
    case 5: //紧急停止
		setArmFlag(0);
        break;
    case 0x10: //上升
        //数据不为零的时候再处理
        if (data[0]) 
        { 
			missionMove(0, 0, 1.0f * data[0],(1 << Z));
			
            //setMoveXYZ(0,0,1.0f * data[0]);
        }
        needZFlag = true;
        break;
    case 0x11: //下降
        if (data[0])
        {
			missionMove(0, 0, -1.0f * data[0],(1 << Z));
            //setMoveXYZ(0,0,-1.0f * data[0]);
        }
        needZFlag = true;
        break;
    case 0x12: //前进
        if (data[0] + data[1])
        {
			missionMove(1.0f * (data[0] + data[1]),0, 0, (1 << X));
            //setMoveXYZ(1.0f * (data[0] + data[1]),0,0);
        }
        needXFlag = true;
        break;
    case 0x13: //后退
        if (data[0] + data[1])
        {
			missionMove(-1.0f* (data[0] + data[1]),0, 0, (1 << X));
        }
        needXFlag = true;
        break;
    case 0x14: //左飞
        if (data[0] + data[1])
        {
			missionMove(0, 1.0f* (data[0] + data[1]),0, (1 << Y));
            //setMoveXYZ(0,-1.0f * (data[0] + data[1]),0);
        }
        needYFlag = true;
        break;
    case 0x15: //右飞
        if (data[0] + data[1])
        {
			missionMove(0,-1.0f* (data[0] + data[1]),0, (1 << Y));
            //setMoveXYZ(0,1.0f * (data[0] + data[1]),0);
        }
        needYFlag = true;
        break;
    case 0x16: //左偏航 
        if (data[0] + data[1]) 
        {
			missionRotate(-1.0f,-1.0f*(data[0] + data[1]));
			//setYawAngle(-1.0f * (data[0] + data[1])); 
        }
        needYawFlag = true;
        break;
    case 0x17: //右偏航
        if (data[0] + data[1])
        {
			missionRotate(1.0f,(data[0] + data[1]));
            //setYawAngle(1.0f * (data[0] + data[1]));
        }
        needYawFlag = true;
        break;
    case 0x20: //前翻
		/**
        if(setFlipDir(FORWARD) == 0)
        {
            flipFailCount = 2;
            recv(recvData, 3);
        }
        else
        {
            needFlipFlag = true;
        }
		**/
        break;
    case 0x21: //后翻
		/**
        if(setFlipDir(BACK) == 0)
        {
            flipFailCount = 2;
            recv(recvData, 3);
        }
        else
        {
            needFlipFlag = true;
        }
		**/
        break;
    case 0x22: //左翻
		/**
        if(setFlipDir(LEFT) == 0)
        {
            flipFailCount = 2;
            recv(recvData, 3);
        }
        else
        {
            needFlipFlag = true;
        }
		**/
        break;
    case 0x23: //右翻
		/**
        if(setFlipDir(RIGHT) == 0)
        {
            flipFailCount = 2;
            recv(recvData, 3);
        }
        else
        {
            needFlipFlag = true;
        }
		**/
        break;
    case 0x30: //指定移动
        {
            float x = (char)data[0] * data[4];
            float y = (char)data[1] * data[4];
            float z = (char)data[2] * data[4];
			missionMove(x, y, z,0xff);
	  
            needXYZFlag = true;
        }
        break;
    case 0x31: //曲线移动

        break;
    default:
        break;
    }
}

static void recv(uint8_t *data, uint8_t len)
{			
	usartMicrobitSend(len,data);		
}

static void updateMicrobitData(uint8_t data)
{
    static uint8_t status;
    static uint8_t index; 
    static uint8_t buff[16];
    static uint8_t length;
    static uint8_t type;	
  //usbsendData(&data, 1);
    static uint8_t beatFlag;
    switch (status)
    {
    case 0:
        if (data == 0xEF) 
        {
            status++; 
        }
        else if (data == 0xAF) 
        {
            beatFlag = 1; 
            status++;
        }
        break;
    case 1:
        if (beatFlag) 
        {
            status = 0;
            beatFlag = 0;
            if (data == 0xFA)  
            {
				receivedHeart=1;
                // 飞机起飞后，开始检测有没有除心跳外的指令发出
                // 执行任务完成后
                // 超过3秒没有则降落
                if (getArmFlag())
                {
                    if (needXFlag == false && needYFlag == false && needZFlag == false && needXYZFlag == false && needLandFlag == false && needFlipFlag == false && needWaitTimeFlag == false && needYawFlag == false && needTakeOffFlag == false)
                    {
                        cmdLiveCount++;
                        if (cmdLiveCount >= 2500)
                        {
                            cmdLiveCount = 0;
							
							setFlightState(CONTROL_MODE_LAND);	 
                        }
                    }
                }
				/**
                if (flipFailCount > 0)
                {
                    flipFailCount--;
                    if (flipFailCount == 0)
                    {
                        if (getStatus() > 8)
                        {
                            setStatus(0);
                        }
                    }
                }
				**/

                // 清除丢包计数
                uint8_t recvData[3] = {1, 1, 0};
                if (needTakeOffFlag)
                {
                    if (getFlightState()==CONTROL_MODE_POSHOLD)//起飞状态
                    {
                        recv(recvData, 3);
                        needTakeOffFlag = 0;
                    }
                }
                if (needXFlag)
                {
                     if (fabs(getErrorPosX()) < 10)
                    {
                        recv(recvData, 3);
                        needXFlag = 0;
                    }
                }
                if (needYFlag)
                {
                    if (fabs(getErrorPosY()) < 10)
                    {
                        recv(recvData, 3);
                        needYFlag = 0;
                    }
                }
                if (needZFlag)
                {
                    if (fabs(getErrorPosZ()) < 10)
                    {
                        recv(recvData, 3);
                        needZFlag = 0;
                    }
                }
                if (needXYZFlag)
                {
                    if ((fabs(getErrorPosX()) < 10) && (fabs(getErrorPosY()) < 10) && (fabs(getErrorPosZ()) < 10))
                    {
                        recv(recvData, 3);
                        needXYZFlag = 0;
                    }
                }
                if (needLandFlag)
                {
                   if(getFlightState()==CONTROL_MODE_STOP)
                   {
                       recv(recvData, 3);
                       needLandFlag = 0;
                   }
                }
                if (needFlipFlag)
                {/**
                    if (isExitFlip == true)
                    {
                        recv(recvData, 3);
                        needFlipFlag = 0;
                    }**/
                }
                if (needWaitTimeFlag)
                {
                    if (waitTime > 0)
                    {
                        waitTime--;
                    } 
                    else
                    {
                        recv(recvData, 3);
                        needWaitTimeFlag = 0;
                    }
                }
                if (needYawFlag)
                {
                    if (fabs(getErrorYaw()) < 5)
                    {
                        recv(recvData, 3);
                        needYawFlag = 0;
                    }
                }
            }
        }
        else
        {
            length = data;
            status++;
        }
        break;
    case 2:				
        type = data; 	
        status++; 		
        break;			
    case 3:				
        // 如果有长度则记录数据
        if (length)
        {
            status++;
            //以最后一个字节记录cmd
            buff[15] = data;
        }
        else
        {
            status = 0;
            if (type == 1)
            {
                //没有则记录要执行的指令
                runCmd(data, NULL);
            }
            else if (type == 2)
            {
                uint8_t bat = getBatVol()/100 ;
                int16_t height = g_pStaEstimator->pos[Z];
                uint8_t recvData[3] = {2};

                switch (data)
                {
                case 1:
                    recvData[1] = bat;
                    recvData[2] = 0;
                    break;
                case 2:
                    if (height > 255)
                    {
                        recvData[1] = 255;
                        recvData[2] = height - 255;
                    }
                    else
                    {
                        recvData[1] = height;
                        recvData[2] = 0;
                    }
                    break;
                default:
                    break;
                }
                recv(recvData, 3);
            }
        }
        break;
    case 4:
        buff[index] = data;
        index++;
        if (index >= length)
        {
            status = 0;
            index = 0;
            length = 0;
            if (type == 1)
            {
                runCmd(buff[15], buff);
            }
        }
        break;
    default:
        break;
    }
}


void uartUpdate_mircobit(void)			
{										
    wIndex = getRemainRecvBuf_mircobit();
    if (rIndex != wIndex)
    {
        int16_t len = wIndex - rIndex;
        if (len < 0)
        {
            len += 256;
        }
        uint16_t i = 0;
        while (i < len)
        {
            uint16_t index = rIndex + i;
            i++;
            if (index > 255)
            {
                index -= 256;
            }
            //uartDataHandle(g_rx_mircobit[index]);
			updateMicrobitData(g_rx_mircobit[index]);
        }
        rIndex = wIndex;
    }
}


void microbitInitRecv(uint8_t flag)
{
    if (flag)
    {
        uint8_t recvData[3] = {0x01, 0x01, 0xAA};
        recv(recvData, 3);
    }
    else
    {
        uint8_t recvData[3] = {0x01, 0x02, 0xAA};
        recv(recvData, 3);
    }
}
uint8_t getHeart(void)
{
	return receivedHeart;
}

